#include <PID_v1.h>

#define LEFT_SENSOR 14
#define CENTER_SENSOR 16
#define RIGHT_SENSOR 18
#define threshold 100

#define MOTOR_A1 5
#define MOTOR_A2 6
#define MOTOR_B1 9
#define MOTOR_B2 10

double Setpoint = 0; // 目标值为0，表示车辆应当保持在中心位置
double Kp = 2;
double Ki = 5;
double Kd = 1;

double Input, Output;
PID pid(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);

int leftSensorValue, centerSensorValue, rightSensorValue;

void setup() {
  Serial.begin(9600);
  pinMode(MOTOR_A1, OUTPUT);
  pinMode(MOTOR_A2, OUTPUT);
  pinMode(MOTOR_B1, OUTPUT);
  pinMode(MOTOR_B2, OUTPUT);

  pid.SetMode(AUTOMATIC);
  pid.SetOutputLimits(-255, 255);

  Serial.println("PID Tuning:");
  Serial.print("Kp: ");
  Serial.println(Kp);
  Serial.print("Ki: ");
  Serial.println(Ki);
  Serial.print("Kd: ");
  Serial.println(Kd);
}

void loop() {
  leftSensorValue = analogRead(LEFT_SENSOR);
  centerSensorValue = analogRead(CENTER_SENSOR);
  rightSensorValue = analogRead(RIGHT_SENSOR);

  // 计算传感器读数的误差，以便作为PID控制器的输入
  double error = ((double)centerSensorValue - (double)rightSensorValue) - ((double)leftSensorValue - (double)centerSensorValue);
  Input = error / 1023.0;

  pid.Compute();

  // 使用PID输出来控制电机转向幅度
  int leftSpeed = constrain(200 + Output, 0, 255); // 左轮速度
  int rightSpeed = constrain(200 - Output, 0, 255); // 右轮速度

  // 控制电机运动
  if (centerSensorValue > threshold) { // 直行
    analogWrite(MOTOR_A1, leftSpeed);
    digitalWrite(MOTOR_A2, LOW);
    analogWrite(MOTOR_B1, rightSpeed);
    digitalWrite(MOTOR_B2, LOW);
  } else if (leftSensorValue > threshold) { // 左转
    analogWrite(MOTOR_A1, 0);
    digitalWrite(MOTOR_A2, leftSpeed);
    analogWrite(MOTOR_B1, rightSpeed);
    digitalWrite(MOTOR_B2, LOW);
  } else if (rightSensorValue > threshold) { // 右转
    analogWrite(MOTOR_A1, leftSpeed);
    digitalWrite(MOTOR_A2, LOW);
    analogWrite(MOTOR_B1, 0);
    digitalWrite(MOTOR_B2, rightSpeed);
  } else { // 停止
    analogWrite(MOTOR_A1, 0);
    digitalWrite(MOTOR_A2, LOW);
    analogWrite(MOTOR_B1, 0);
    digitalWrite(MOTOR_B2, LOW);
  }

  // 输出调试信息
  Serial.print("Input: ");
  Serial.print(Input);
  Serial.print(" Output: ");
  Serial.println(Output);

  delay(10);
}
